﻿using UnityEngine;

public static class Vector3Extensions {

	public static Vector3 GetRelativePositionFrom(this Vector3 position, Matrix4x4 from) {
		// Returns a position v transformed by the current fully arbitrary matrix.
		// Vector3f res;
		// res.x = m_Data[0] * v.x + m_Data[4] * v.y + m_Data[ 8] * v.z + m_Data[12];
		// res.y = m_Data[1] * v.x + m_Data[5] * v.y + m_Data[ 9] * v.z + m_Data[13];
		// res.z = m_Data[2] * v.x + m_Data[6] * v.y + m_Data[10] * v.z + m_Data[14];
		// return res;
		return from.MultiplyPoint(position);
	}

	public static Vector3 GetRelativePositionTo(this Vector3 position, Matrix4x4 to) {
		return to.inverse.MultiplyPoint(position);
	}

	public static Vector3 GetRelativeDirectionFrom(this Vector3 direction, Matrix4x4 from) {
		// This function is similar to MultiplyPoint; but it transforms directions and not positions.
		// When transforming a direction, only the rotation part of the matrix is taken into account.
		// Vector3f res;
		// res.x = m_Data[0] * v.x + m_Data[4] * v.y + m_Data[ 8] * v.z;
		// res.y = m_Data[1] * v.x + m_Data[5] * v.y + m_Data[ 9] * v.z;
		// res.z = m_Data[2] * v.x + m_Data[6] * v.y + m_Data[10] * v.z;
		// return res;
		return from.MultiplyVector(direction);
	}

	public static Vector3 GetRelativeDirectionTo(this Vector3 direction, Matrix4x4 to) {
		return to.inverse.MultiplyVector(direction);
	}

	public static Vector3 GetMirror(this Vector3 vector, Vector3 axis) {
		if(axis == Vector3.right) {
			vector.x *= -1f;
		}
		if(axis == Vector3.up) {
			vector.y *= -1f;
		}
		if(axis == Vector3.forward) {
			vector.z *= -1f;
		}
		return vector;
	}
}
